//
// Created by HP on 2024/4/15.
//

#include "aurora_motor.h"



Aurora_Motor::Aurora_Motor(){
    focL.set_tim(&htim1,TIM_CHANNEL_1,TIM_CHANNEL_2,TIM_CHANNEL_3,TIM_CHANNEL_4);
    focL.set_adc(&hadc1);

    focR.set_tim(&htim8,TIM_CHANNEL_1,TIM_CHANNEL_2,TIM_CHANNEL_3,TIM_CHANNEL_4);
    focR.set_adc(&hadc2);


    state.ctrlState = IDLE;

    Send_Detail_cnt = 0;
    Send_Detail_Flag = DISABLE;
}

void Aurora_Motor::add_AS5047_Driver(AS5047_Driver *as5047_L,AS5047_Driver *as5047_R){
    focL.set_endocer(as5047_L);
    focR.set_endocer(as5047_R);
}
void Aurora_Motor::start_pwm(){
    focL.start_pwm();
    focR.start_pwm();
}
//校准电机 和 极对数
void  Aurora_Motor::calibration_angle_pole_pair(uint8_t enable_L,uint8_t enable_R){
    if( enable_L == ENABLE){
        focL.calibration_angle_pole_pair();
        focL.halt_pwm();
    }
    if( enable_R == ENABLE ) {
        focR.calibration_angle_pole_pair();
        focR.halt_pwm();
    }
}
//使能电流采样
void Aurora_Motor::enable_current_sampling(uint8_t enable_L,uint8_t enable_R){
    if( enable_L == ENABLE )
        focL.start_adc_sample();
    if( enable_R == ENABLE )
        focR.start_adc_sample();
}
// 开环状态下
void Aurora_Motor::open_deal(){

}
//主控制
void Aurora_Motor::main_deal(uint8_t enable_L,uint8_t enable_R,uint32_t data1,uint32_t data2,uint32_t data3,uint32_t data4){
    if( enable_L == ENABLE){
        if( state.ctrlState == LOPEN_MODE || state.ctrlState == LROPEN_MODE) {
            focL.update_I(2, data1, data2);
            focL.ctrl_open_Loop();
            foc2ui_msg.detail_data.LMode    =  1;
            foc2ui_msg.detail_data.LDesire  =  focL.Uq;

        }
        else if( state.ctrlState == LI_MODE || state.ctrlState == LRI_MODE){
            focL.update_I(2, data1, data2);
            focL.ctrl_I_Loop();
            foc2ui_msg.detail_data.LMode    =  2;
            foc2ui_msg.detail_data.LDesire  =  focL.I_pid_q._desired_value;
        }
        else if( state.ctrlState == LSPEED_MODE || state.ctrlState == LRSPEED_MODE){
            focL.update_I(2, data1, data2);
            focL.ctrl_speed_Loop();
            foc2ui_msg.detail_data.LMode    =  3;
            foc2ui_msg.detail_data.LDesire  =  focL.Speed_pid._desired_value;
        }
        else if( state.ctrlState == LPOSITION_MODE || state.ctrlState == LRPOSITION_MODE){
            focL.update_I(2, data1, data2);
            focL.ctrl_position_Loop();
            foc2ui_msg.detail_data.LMode    =  4;
            foc2ui_msg.detail_data.LDesire  =  0;
        }
        else{
            foc2ui_msg.detail_data.LMode    =  0;
        }
    }
    if( enable_R == ENABLE){
        if( state.ctrlState == ROPEN_MODE || state.ctrlState == LROPEN_MODE) {
            focR.update_I(2, data3, data4);
            focR.ctrl_open_Loop();
            foc2ui_msg.detail_data.RMode    =  1;
            foc2ui_msg.detail_data.RDesire  =  focR.Uq;
        }
        else if( state.ctrlState == RI_MODE || state.ctrlState == LRI_MODE) {
            focR.update_I(2, data3, data4);
            focR.ctrl_I_Loop();
            foc2ui_msg.detail_data.RMode    =  2;
            foc2ui_msg.detail_data.RDesire  =  focR.I_pid_q._desired_value;
        }
        else if( state.ctrlState == RSPEED_MODE || state.ctrlState == LRSPEED_MODE) {
            focR.update_I(2, data3, data4);
            focR.ctrl_speed_Loop();
            foc2ui_msg.detail_data.RMode    =  3;
            foc2ui_msg.detail_data.RDesire  =  focR.Speed_pid._desired_value;
        }
        else if( state.ctrlState == RPOSITION_MODE || state.ctrlState == LRPOSITION_MODE){
            focR.update_I(2, data3, data4);
            focR.ctrl_position_Loop();
            foc2ui_msg.detail_data.RMode    =  4;
            foc2ui_msg.detail_data.RDesire  =  0;
        }
        else{
            foc2ui_msg.detail_data.RMode    =  0;
        }
    }

    if( Send_Detail_Flag == ENABLE){
        Send_Detail_cnt += 1;
        if( Send_Detail_cnt < 10000)
            return;
        Send_Detail_cnt = 0;
        foc2ui_msg.detail_data.LUq      = focL.Uq;
        foc2ui_msg.detail_data.LCurrent = focL.Iq;
        foc2ui_msg.detail_data.LSpeed   = focL.speed;
        foc2ui_msg.detail_data.LPosition= focL.mechanical_angle;

        foc2ui_msg.detail_data.RUq      =  focR.Uq;
        foc2ui_msg.detail_data.RCurrent =  focR.Iq;
        foc2ui_msg.detail_data.RSpeed   =  focR.speed;
        foc2ui_msg.detail_data.RPosition=  focR.mechanical_angle;

        foc2ui_msg.type = update_detail;
        foc2ui_msg.cmd  = IS_CMD;

    }
}


void Aurora_Motor::dealmsgEvent_from_UI(struct Aurora_UI2FOC_MSG *ui2foc_msg_p){
    if( ui2foc_msg_p->type == enable_mode_Lmotor){
        if( ui2foc_msg_p->foc_mode == Calibration_MOTOR)
            calibration_angle_pole_pair(ENABLE,DISABLE);
        else if(  ui2foc_msg_p->foc_mode == HALT_MOTOR)
            state.ctrlState = IDLE;
        else if(  ui2foc_msg_p->foc_mode == OPEN_LOOP_MOTOR)
            state.ctrlState = LOPEN_MODE;
        else if(  ui2foc_msg_p->foc_mode == I_LOOP_MOTOR)
            state.ctrlState = LI_MODE;
        else if( ui2foc_msg_p->foc_mode == SPEED_LOOP_MOTOR)
            state.ctrlState = LSPEED_MODE;
        else if( ui2foc_msg_p->foc_mode == POSITION_LOOP_MOTOR)
            state.ctrlState = LPOSITION_MODE;
    }
    else if( ui2foc_msg_p->type == enable_mode_Rmotor){
        if( ui2foc_msg_p->foc_mode == Calibration_MOTOR)
            calibration_angle_pole_pair(DISABLE,ENABLE);
        else if(  ui2foc_msg_p->foc_mode == HALT_MOTOR)
            state.ctrlState = IDLE;
        else if(  ui2foc_msg_p->foc_mode == OPEN_LOOP_MOTOR)
            state.ctrlState = ROPEN_MODE;
        else if(  ui2foc_msg_p->foc_mode == I_LOOP_MOTOR)
            state.ctrlState = RI_MODE;
        else if( ui2foc_msg_p->foc_mode == SPEED_LOOP_MOTOR)
            state.ctrlState = RSPEED_MODE;
        else if( ui2foc_msg_p->foc_mode == POSITION_LOOP_MOTOR)
            state.ctrlState = RPOSITION_MODE;

    }
    else if( ui2foc_msg_p->type == enable_mode_LRmotor){
        if( ui2foc_msg_p->foc_mode == Calibration_MOTOR)
            calibration_angle_pole_pair(ENABLE,ENABLE);
        else if(  ui2foc_msg_p->foc_mode == HALT_MOTOR)
            state.ctrlState = IDLE;
        else if(  ui2foc_msg_p->foc_mode == OPEN_LOOP_MOTOR)
            state.ctrlState = LROPEN_MODE;
        else if(  ui2foc_msg_p->foc_mode == I_LOOP_MOTOR)
            state.ctrlState = LRI_MODE;
        else if( ui2foc_msg_p->foc_mode == SPEED_LOOP_MOTOR)
            state.ctrlState = LRSPEED_MODE;
        else if( ui2foc_msg_p->foc_mode == POSITION_LOOP_MOTOR)
            state.ctrlState = LRPOSITION_MODE;
    }

    else if( ui2foc_msg_p->type == enable_wave ){
        switch (ui2foc_msg_p->wave_type) {
            case LFOC_TaTbTc:
                focL.set_send_wave_type(Aurora_FOC::SEND_FOC_TaTbTc);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);
                break;
            case LFOC_IaIbIc:
                focL.set_send_wave_type(Aurora_FOC::SEND_FOC_IaIbIc);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);break;
            case LFOC_IqId:
                focL.set_send_wave_type(Aurora_FOC::SEND_FOC_IqId);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);break;
            case LFOC_IalphaIbeta:
                focL.set_send_wave_type(Aurora_FOC::SEND_FOC_IalphaIbeta);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);break;
            case LFOC_Position:
                focL.set_send_wave_type(Aurora_FOC::SEND_FOC_Position);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);break;

            case RFOC_TaTbTc:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_FOC_TaTbTc);break;
            case RFOC_IaIbIc:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_FOC_IaIbIc);break;
            case RFOC_IqId:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_FOC_IqId);break;
            case RFOC_IalphaIbeta:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_FOC_IalphaIbeta);break;
            case RFOC_Position:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_FOC_Position);break;
            case NONE:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);
                break;
            default:
                focL.set_send_wave_type(Aurora_FOC::SEND_NONE);
                focR.set_send_wave_type(Aurora_FOC::SEND_NONE);
                break;
        }
    }
    else if(ui2foc_msg_p->type == enable_detail){
        Send_Detail_Flag = ENABLE;
    }
    else if(ui2foc_msg_p->type == disable_detail){
        Send_Detail_Flag = DISABLE;
    }

}


void Aurora_Motor::dealmsgEvent_from_COMM(struct Aurora_COMM2FOC_MSG *comm2foc_msg_p){
    if( comm2foc_msg_p->type == commenable_LMCalibration) {
        state.ctrlState = IDLE;
        calibration_angle_pole_pair(ENABLE, DISABLE);
    }
    else if( comm2foc_msg_p->type == commenable_RMCalibration) {
        state.ctrlState = IDLE;
        calibration_angle_pole_pair(DISABLE, ENABLE);

    }
    else if( comm2foc_msg_p->type == commenable_LRMCalibration ) {
        state.ctrlState = IDLE;
        calibration_angle_pole_pair(ENABLE, ENABLE);
    }
    else if( comm2foc_msg_p->type == LEFT_MOTOR){
        if(comm2foc_msg_p->mode == OPEN_MODE)
            state.ctrlState = LOPEN_MODE;
        else if( comm2foc_msg_p->mode == I_MODE)
            state.ctrlState = LI_MODE;
        else if( comm2foc_msg_p->mode == SPEED_MODE)
            state.ctrlState = LSPEED_MODE;
        else if( comm2foc_msg_p->mode == POSITION_MODE)
            state.ctrlState = LPOSITION_MODE;
    }
    else if( comm2foc_msg_p->type == RIGHT_MOTOR){
        if(comm2foc_msg_p->mode == OPEN_MODE)
            state.ctrlState = ROPEN_MODE;
        else if( comm2foc_msg_p->mode == I_MODE)
            state.ctrlState = RI_MODE;
        else if( comm2foc_msg_p->mode == SPEED_MODE)
            state.ctrlState = RSPEED_MODE;
        else if( comm2foc_msg_p->mode == POSITION_MODE)
            state.ctrlState = RPOSITION_MODE;
    }
    else if( comm2foc_msg_p->type == BOTH_MOTOR){
        if(comm2foc_msg_p->mode == OPEN_MODE)
            state.ctrlState = LROPEN_MODE;
        else if(comm2foc_msg_p->mode == I_MODE)
            state.ctrlState = LRI_MODE;
        else if(comm2foc_msg_p->mode == SPEED_MODE)
            state.ctrlState = LRSPEED_MODE;
        else if( comm2foc_msg_p->mode == POSITION_MODE)
            state.ctrlState = LRPOSITION_MODE;
    }
    else if(comm2foc_msg_p->type == HALT_BOTH_MOTOR){
        focL.halt_pwm();
        focR.halt_pwm();
        state.ctrlState = IDLE;
    }
    else if( comm2foc_msg_p->type == SET_UqUd){
        if( comm2foc_msg_p->mode == LUqUd)
            focL.set_UqUd(comm2foc_msg_p->Uq,comm2foc_msg_p->Ud);
        else
            focR.set_UqUd(comm2foc_msg_p->Uq,comm2foc_msg_p->Ud);
    }
    else if( comm2foc_msg_p->type == SET_WAVEMAXMIN){
        if( comm2foc_msg_p->mode == LTaTbTc_MAXMIN)
            focL.set_Wave_TaTbTc_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else if( comm2foc_msg_p->mode == LIaIbIc_MAXMIN)
            focL.set_Wave_IaIbIc_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else if( comm2foc_msg_p->mode == LIqId_MAXMIN)
            focL.set_Wave_IqId_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else if( comm2foc_msg_p->mode == LIalphaIbeta_MAXMIN)
            focL.set_Wave_IalphaIbeta_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else if( comm2foc_msg_p->mode == RTaTbTc_MAXMIN)
            focR.set_Wave_TaTbTc_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else if( comm2foc_msg_p->mode == RIaIbIc_MAXMIN)
            focR.set_Wave_IaIbIc_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else if( comm2foc_msg_p->mode == RIqId_MAXMIN)
            focR.set_Wave_IqId_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
        else
            focR.set_Wave_IalphaIbeta_MAXMIN(comm2foc_msg_p->wave_max,comm2foc_msg_p->wave_min);
    }
    else if(comm2foc_msg_p->type == SET_I_PARAM){
        if(comm2foc_msg_p->mode == LI_AIM)
            focL.I_pid_q.set_desireValue(comm2foc_msg_p->pid_parm.AIM);
        else if(comm2foc_msg_p->mode == LI_Qpi)
            focL.I_pid_q.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
        else if(comm2foc_msg_p->mode == LI_Dpi)
            focL.I_pid_d.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
        else if(comm2foc_msg_p->mode == RI_AIM)
            focR.I_pid_q.set_desireValue(comm2foc_msg_p->pid_parm.AIM);
        else if(comm2foc_msg_p->mode == RI_Qpi)
            focR.I_pid_q.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
        else if(comm2foc_msg_p->mode == RI_Dpi)
            focR.I_pid_d.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
    }
    else if(comm2foc_msg_p->type == SET_SPEED_PARAM){
        if( comm2foc_msg_p->mode == LSPEED_AIM)
            focL.Speed_pid.set_desireValue(comm2foc_msg_p->pid_parm.AIM);
        else if(comm2foc_msg_p->mode == LSPEED_PI)
            focL.Speed_pid.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
        else if(comm2foc_msg_p->mode == RSPEED_AIM)
            focR.Speed_pid.set_desireValue(comm2foc_msg_p->pid_parm.AIM);
        else if(comm2foc_msg_p->mode == RSPEED_PI)
            focR.Speed_pid.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
    }
    else if(comm2foc_msg_p->type == SET_POSITION_PARAM){
        if( comm2foc_msg_p->mode == LPOSITION_AIM)
            focL.Position_pid.set_desireValue(comm2foc_msg_p->pid_parm.AIM);
        else if(comm2foc_msg_p->mode == LPOSITION_PI)
            focL.Position_pid.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
        else if(comm2foc_msg_p->mode == RPOSITION_AIM)
            focR.Position_pid.set_desireValue(comm2foc_msg_p->pid_parm.AIM);
        else if(comm2foc_msg_p->mode == RPOSITION_PI)
            focR.Position_pid.set_PID(comm2foc_msg_p->pid_parm.P,comm2foc_msg_p->pid_parm.I,0);
    }


}